#include "MotorsTask.h"
#include <string.h>
#include "dac.h"
#include "udp_task.h"
#include "tcp_transfer.h"
/*
电机所有错误都使用该字符串上传
当前电机驱动错误包括：复位异常中途停止、电机在复位或运动中异常上下极限位置堵转、
电机速度异常变低且无法
*/
static uint32_t stall_flag = 0;/*堵转时间flag*/
/*运动开始的系统时间*/
static uint32_t home_tick = 0;
static uint32_t jog_tick = 0;
#define MOTOR_HOME_TIMEOUT	30000
#define MOTOR_RUN_TIMEOUT	15000 /*任何电机运动超过就算超时*/
static char MOTOR_ERR_STALL[] = "计数仪电机驱动错误!";
static char MOTOR_ERR_TIMEOUT[] = "计数仪电机超时错误!";
static char MOTOR_ERR_BSP[] = "计数仪硬件错误!";

transfer_err_t tcp_cmd(protocol_cmd_t cmd,void* buff,uint16_t size);
void motor_poll(TypeDefTMC4361_RegStruct* handle);
void motor_home(TypeDefTMC4361_RegStruct* handle);
void motor_tarpos(TypeDefTMC4361_RegStruct* handle,int32_t tar);
void motor_relativepos(TypeDefTMC4361_RegStruct* handle,int32_t tar);
void motor_speed(TypeDefTMC4361_RegStruct* handle,int32_t speed);
uint32_t startTick, endTick, elapsedTime;
TypeDefTMC4361_RegStruct* handleX = &TMC4361_M1RegStruct;
TypeDefTMC4361_RegStruct* handleY = &TMC4361_M2RegStruct;
TypeDefTMC4361_RegStruct* handleZ = &TMC4361_M3RegStruct;
uint8_t HOMED_R=1;
void MotorsTask(void *pvParameters)
{
	
	static int32_t input1 = 0;
	static int32_t input2 = 0;
	static uint8_t btn1 = 0,btn2 = 0;
	uint16_t btn_msg[2];
	btn1 = HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_10);
	btn2 = HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_11);
	tcp_transfer_init(tcp_cmd);
	uint8_t recvData[50];
	Send_Data SendData;
	float LED_Light = 0; 
	M1_Init();
	M2_Init();
	M3_Init();
	UDP_Send_Data sendData;
	char TCP_message[128];
	while(1)
	{
		taskENTER_CRITICAL();
		motor_poll(handleX);
		motor_poll(handleY);
		motor_poll(handleZ);
		taskEXIT_CRITICAL();
		/*检测堵转错误*/
		static uint8_t stall_flag = 0;
		if(
			(stall_flag == 0) &&
			( (handleX->PROTO_STATUS & M_STATUS_STALL)
			|| (handleY->PROTO_STATUS & M_STATUS_STALL)
			|| (handleY->PROTO_STATUS & M_STATUS_STALL))
		)
		{
			stall_flag = 1;
			tcp_transfer_push(RPLY_ERROR,MOTOR_ERR_STALL,strlen(MOTOR_ERR_STALL));
		}
		/*复位过程中 检测超时*/
		if(HOMED_R==0 && (handleX->HOMING_FLAG == 0 || handleY->HOMING_FLAG == 0 || handleZ->HOMING_FLAG == 0) )
		{
			if((xTaskGetTickCount() - home_tick) > MOTOR_HOME_TIMEOUT)
			{
				home_tick = xTaskGetTickCount();
				tcp_transfer_push(RPLY_ERROR,MOTOR_ERR_TIMEOUT,strlen(MOTOR_ERR_TIMEOUT));
			}
		}
		/*位置运动超时检测*/
		if( 
			(handleX->MOVING_FLAG == 1 && (((handleX->PROTO_STATUS & 0x28) != 0x28)))
			 || (handleY->MOVING_FLAG == 1 && (((handleY->PROTO_STATUS & 0x28) != 0x28)))
			 || (handleZ->MOVING_FLAG == 1 && (((handleZ->PROTO_STATUS & 0x28) != 0x28)))
		)
		{
			if((xTaskGetTickCount() - jog_tick) > MOTOR_RUN_TIMEOUT)
			{
				jog_tick = xTaskGetTickCount();
				tcp_transfer_push(RPLY_ERROR,MOTOR_ERR_TIMEOUT,strlen(MOTOR_ERR_TIMEOUT));
			}
		}
	if(HOMED_R==0&&handleX->HOMING_FLAG==1
		&&handleY->HOMING_FLAG==1&&handleZ->HOMING_FLAG==1
	){
		
		tcp_transfer_push(RPLY_HOME,NULL,0);
			HOMED_R=1;
			}
	uint8_t x_arrived = (((handleX->PROTO_STATUS & 0x28) == 0x28));
	uint8_t y_arrived = (((handleY->PROTO_STATUS & 0x28) == 0x28));
	uint8_t z_arrived = (((handleZ->PROTO_STATUS & 0x28) == 0x28));
	if (handleX->MOVING_FLAG == 1 && x_arrived)
	{
		handleX->MOVING_FLAG = 0;
		handleX->POLLING_FLAG = 0;
		uint16_t id = CHANEL_1;
		tcp_transfer_push(RPLY_JOG,&id,2);
	}
	if (handleY->MOVING_FLAG == 1 && y_arrived)
	{
		handleY->MOVING_FLAG = 0;
		handleY->POLLING_FLAG = 0;
		uint16_t id = CHANEL_2;
		tcp_transfer_push(RPLY_JOG,&id,2);
	}
	if (handleZ->MOVING_FLAG == 1 && z_arrived)
	{
		handleZ->MOVING_FLAG = 0;
		handleZ->POLLING_FLAG = 0;
		uint16_t id = CHANEL_3;
		tcp_transfer_push(RPLY_JOG,&id,2);
	}
	/*按键输入*/
	if(GPIO_PIN_RESET == HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_10))
	{input1--;}
	else
	{input1++;}
	if(GPIO_PIN_RESET == HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_11))
	{input2--;}
	else
	{input2++;}
	
	if(input1 > 5)
	{
		input1 = 5;
		if(btn1 == 0)
		{
			btn1 = 1;
			btn_msg[0] = 0;
			btn_msg[1] = btn1;
			tcp_transfer_push(RPLY_BTN,&btn_msg,sizeof(btn_msg));
		}
	}
	if(input1 < -5)
	{
		input1 = -5;
		if(btn1 == 1)
		{
			btn1 = 0;
			btn_msg[0] = 0;
			btn_msg[1] = btn1;
			tcp_transfer_push(RPLY_BTN,&btn_msg,sizeof(btn_msg));
		}
	}
	
	if(input2 > 5)
	{
		input2 = 5;
		if(btn2 == 0)
		{
			btn2 = 1;
			btn_msg[0] = 1;
			btn_msg[1] = btn2;
			tcp_transfer_push(RPLY_BTN,&btn_msg,sizeof(btn_msg));
		}
	}
	if(input2 < -5)
	{
		input2 = -5;
		if(btn2 == 1)
		{
			btn2 = 0;
			btn_msg[0] = 1;
			btn_msg[1] = btn2;
			tcp_transfer_push(RPLY_BTN,&btn_msg,sizeof(btn_msg));
		}
	}
	osDelay(10);
	}
}

transfer_err_t tcp_cmd(protocol_cmd_t cmd,void* buff,uint16_t size)
{
	taskENTER_CRITICAL();
//		if (osMessageQueueGet(motorQueueHandle, &recvData, NULL, 100) == osOK) { 
//			startTick=HAL_GetTick();
	int32_t X_Coord = 0,Y_Coord = 0,Z_Coord = 0;
	jog_cmd_t* p_jog = (jog_cmd_t*)buff;
		switch(cmd)
		{
			case CMD_HOME :{
				if(handleX->MOVING_FLAG==0&&handleY->MOVING_FLAG==0&&handleZ->MOVING_FLAG==0){
				handleX->HOMING_FLAG=0;
				handleY->HOMING_FLAG=0;
				handleZ->HOMING_FLAG=0;
#ifdef PRODUCT_NEW
				handleX->HOMEING_CMD=0X02;
				handleY->HOMEING_CMD=0X04;
				handleZ->HOMEING_CMD=0X04;
#else
				handleX->HOMEING_CMD=0X04;
				handleY->HOMEING_CMD=0X02;
				handleZ->HOMEING_CMD=0X04;
#endif
#ifdef PRODUCT_NEW
				motor_home(handleY);
				motor_home(handleZ);
				motor_home(handleX);
#else
				motor_home(handleZ);
				motor_home(handleX);
				motor_home(handleY);
#endif
				home_tick = xTaskGetTickCount();
				HOMED_R=0;}
				break; 
			case CMD_JOG :{
				jog_tick = xTaskGetTickCount();
				if(p_jog->id == CHANEL_1 && p_jog != NULL){
					handleX->MOVING_FLAG  = 1;
					if(p_jog->speed != 0)
						motor_speed(handleX,p_jog->speed);
					if(p_jog->relative_pos == 1)
					{
						motor_relativepos(handleX,-p_jog->pos);
					}
					else{
						motor_tarpos(handleX,-p_jog->pos);
					}
					handleX->POLLING_FLAG=1;
				}
				if(p_jog->id == CHANEL_2 && p_jog != NULL){
					handleY->MOVING_FLAG  = 1;
					if(p_jog->speed != 0)
						motor_speed(handleY,p_jog->speed);
					if(p_jog->relative_pos == 1)
					{
						motor_relativepos(handleY,-p_jog->pos);
					}
					else{
						motor_tarpos(handleY,-p_jog->pos);
					}
					handleY->POLLING_FLAG=1;
				}
				if(p_jog->id == CHANEL_3 && p_jog != NULL){
					handleZ->MOVING_FLAG  = 1;
					if(p_jog->speed != 0)
						motor_speed(handleZ,p_jog->speed);
					if(p_jog->relative_pos == 1)
					{
						motor_relativepos(handleZ,-p_jog->pos);
					}
					else{
						motor_tarpos(handleZ,-p_jog->pos);
					}
					handleZ->POLLING_FLAG=1;
				}
			}									
				break;  // 添加break语句
					}
			case CMD_LED:
				if(*((uint16_t*)buff) == 0)
				{
					if(*((uint16_t*)buff + 1))
					{
						HAL_GPIO_WritePin(GPIOE, LED1_Pin, GPIO_PIN_SET);
					}
					else
						HAL_GPIO_WritePin(GPIOE, LED1_Pin, GPIO_PIN_RESET);
				}
				else if(*((uint16_t*)buff) == 1)
				{
					if(*((uint16_t*)buff + 1))
					{
						HAL_GPIO_WritePin(GPIOE, LED2_Pin, GPIO_PIN_SET);
					}
					else
						HAL_GPIO_WritePin(GPIOE, LED2_Pin, GPIO_PIN_RESET);
				}
			default:
        break;
			}
		
	taskEXIT_CRITICAL();
}
void M_STATUS_SET(uint32_t* status,uint32_t flag)
{
	(*status) = (*status) | flag;
}
void M_STATUS_RESET(uint32_t* status,uint32_t flag)
{
	(*status) = (*status) & (~flag);
}

void motor_home(TypeDefTMC4361_RegStruct* handle)
{
	uint32_t TempDat;
	if(handle->HOMEING_CMD == 0)
		return;

	switch(handle->HOMEING_CMD)
	{
		case 0X01 : {  //  电机停止回原点			
		}break;
		case 0X02 : {  //  电机向右旋转回原点高电平停止电机
			handle->bsp_write_reg(0X24,0);       // VMAX
			handle->bsp_write_reg(0X20,0X00);       //     速度模式
			handle->bsp_write_reg(0X01,0X00);  // 清空限位
			handle->bsp_write_reg(0X01,STOPR_EN | STOPR_H_EN | STOPR_E_XLATCH);  // 使能右限位有效  使能高电平有效停止
			handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	    handle->STATUS= TempDat;
			if((TempDat & STOPR_ACTIVE) == STOPR_ACTIVE)  // 判断光电为高电平，需要反向运动实现光电为低电平
			{
//				handle->LIMIT_SWITCH_CMD=0x03;
//				limit_switch(handle);
				handle->bsp_write_reg(0X01,0X00);  // 清空限位
				handle->bsp_write_reg(0X01,STOPL_EN | STOPL_L_EN | 0X00000010);  // 使能左限位有效  使能低电平有效停止
				handle->bsp_write_reg(0X24,(int32_t)(-FAST_HOME_SPEED));       // 启动电机左转
				handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				Delayms(1);
//				while(TempDat == 0)
//				{
//					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				}
				handle->HOMEING_CMD = TYPE2_FAST_LEAVE;//06
			}
			else    // 判断光电为低电平，直接复位
			{
				
				handle->bsp_write_reg(0X24,FAST_HOME_SPEED);       // 启动电机右转
				handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
				handle->STATUS= TempDat;
//				Delayms(1);
//				while(TempDat == 0)
//				{
//					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				}
				handle->HOMEING_CMD = TYPE2_FAST_REACH;//20
			}
		}break;
		case 0X03 : {  //  电机向右旋转回原点低电平停止电机

		}break;
		case 0X04 : {  //  电机向左旋转回原点高电平停止电机
			handle->bsp_write_reg(0X24,0);       // VMAX
			handle->bsp_write_reg(0X20,0X00);       //     速度模式
			handle->bsp_write_reg(0X01,0X00);  // 清空限位
			handle->bsp_write_reg(0X01,STOPL_EN | STOPL_H_EN | STOPL_E_XLATCH);  // 使能左限位有效  使能高电平有效停止
			handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	    handle->STATUS= TempDat;
			if((TempDat & STOPL_ACTIVE) == STOPL_ACTIVE)  // 判断光电为高电平，需要反向运动实现光电为低电平
			{
				handle->bsp_write_reg(0X01,0X00);  // 清空限位
				handle->bsp_write_reg(0X01,STOPR_EN | STOPR_L_EN | 0X00000010);  // 使能右限位有效  使能低电平有效停止
				handle->bsp_write_reg(0X24,FAST_HOME_SPEED);       // 启动电机右转
				handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				Delayms(1);
//				while(TempDat == 0)
//				{
//					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				}
				handle->HOMEING_CMD = TYPE4_FAST_LEAVE;//08
			}
			else    // 判断光电为低电平，直接复位
			{
				handle->bsp_write_reg(0X24,(int32_t)(-FAST_HOME_SPEED));       // 启动电机左转
				handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
				handle->STATUS = (int32_t)TempDat;
//				Delayms(1);
//				while(TempDat == 0)
//				{
//					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//				}
				handle->HOMEING_CMD = TYPE4_FAST_REACH;//30
			}			
		}break;
		case 0X05 : {  //  电机向左旋转回原点低电平停止电机
			
		}break;
		default : break;
	}
}

static void HomingUpdate(TypeDefTMC4361_RegStruct *handle)
{
	uint32_t TempDat = 0;
	if(handle->HOMING_FLAG == 0)
//	if((handle->PROTO_STATUS&M_STATUS_HOME_FINISH) == 0)
	{
		switch(handle->HOMEING_CMD)
		{
			case TYPE2_FAST_REACH:
				handle->stall_timer=0;
			  handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	      handle->STATUS= TempDat;
				if((TempDat & STOPR_ACTIVE) == STOPR_ACTIVE)
				{
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					if(TempDat == 0)
					{
						handle->bsp_write_reg(0X24,0);       // VMAX
						handle->bsp_write_reg(0X01,0X00);  // 清空限位
						handle->bsp_write_reg(0X01,STOPL_EN | STOPL_L_EN | 0X00000010);  // 使能左限位有效  使能低电平有效停止
						handle->bsp_write_reg(0X24,(int32_t)(-FAST_HOME_SPEED));       // 启动电机左转
						handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//						Delayms(1);
						
						handle->HOMEING_CMD = TYPE2_FAST_LEAVE;
					}
				}
			  break;
			case TYPE2_FAST_LEAVE:
				handle->stall_timer=0;
				handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
				handle->STATUS= TempDat;
				if((TempDat & STOPL_ACTIVE) == STOPL_ACTIVE)  // 判断光电为高电平，需要反向运动实现光电为低电平
				{
					handle->bsp_write_reg(0X24,0);       // 
					handle->bsp_write_reg(0X01,0X00);  // 清空限位
					handle->bsp_write_reg(0X01,STOPR_EN | STOPR_H_EN | STOPR_E_XLATCH);  // 使能右限位有效  使能高电平有效停止
					handle->bsp_write_reg(0X24,SLOW_HOME_SPEED);       // 启动电机右转
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//					Delayms(1);
				//					 while(TempDat == 0)
				//						{
				//							handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				//						}
					handle->HOMEING_CMD = TYPE2_SLOW_REACH;
				}
			  break;
			case TYPE2_SLOW_REACH:
				handle->stall_timer=0;
			  handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	      handle->STATUS= TempDat;
				if((TempDat & STOPR_ACTIVE) == STOPR_ACTIVE)
				{
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					if(TempDat == 0)
					{
						handle->bsp_write_reg(0X24,0);       // VMAX
						handle->bsp_write_reg(0X01,0X00);  // 清空限位
						handle->bsp_read_reg(0X21,&TempDat);  // 读取实际位置
						handle->bsp_write_reg(0X37,TempDat);   // 目标位置等于实际位置
//						Delayms(2);
						handle->bsp_write_reg(0X20,0X06);      //     位置模式
						handle->bsp_write_reg(0X24,PROTO_V_TO_4361_REG(handle->PROTO_V));       // VMAX	
						handle->SPI_Status = handle->bsp_read_reg(0X36,&TempDat);
						handle->X_LATCH = (int32_t)TempDat;
						handle->XTARGET = handle->X_LATCH;
						handle->HOMEING_CMD = 0;
						handle->HOMING_FLAG = 1;
//						handle->PROTO_STATUS=(handle->PROTO_STATUS|M_STATUS_HOME_FINISH);
					}
				}
			  break;
			case TYPE4_FAST_REACH:
				handle->stall_timer=0;
			  handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	      handle->STATUS= TempDat;
				if((TempDat & STOPL_ACTIVE) == STOPL_ACTIVE)
				{
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					if(TempDat == 0)
					{
						handle->bsp_write_reg(0X01,0X00);  // 清空限位
						handle->bsp_write_reg(0X01,STOPR_EN | STOPR_L_EN | 0X00000010);  // 使能右限位有效  使能低电平有效停止
						handle->bsp_write_reg(0X24,FAST_HOME_SPEED);       // 启动电机右转
						handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//						Delayms(1);
						
						handle->HOMEING_CMD = TYPE4_FAST_LEAVE;
					}
				}
			  break;
			case TYPE4_FAST_LEAVE:
				handle->stall_timer=0;
				handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
				handle->STATUS= TempDat;
				if((TempDat & STOPR_ACTIVE) == STOPR_ACTIVE)  // 判断光电为高电平，需要反向运动实现光电为低电平
				{
					handle->bsp_write_reg(0X24,0);       // VMAX
					handle->bsp_write_reg(0X01,0X00);  // 清空限位
					handle->bsp_write_reg(0X01,STOPL_EN | STOPL_H_EN | STOPL_E_XLATCH);  // 使能右限位有效  使能高电平有效停止
					handle->bsp_write_reg(0X24,(int32_t)(-SLOW_HOME_SPEED));       // 启动电机右转
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
//					Delayms(1);
				//					 while(TempDat == 0)
				//						{
				//							handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
				//						}
					handle->HOMEING_CMD = TYPE4_SLOW_REACH;
				}
			  break;
			case TYPE4_SLOW_REACH:
				handle->stall_timer=0;
			  handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
	      handle->STATUS= TempDat;
				if((TempDat & STOPL_ACTIVE) == STOPL_ACTIVE)
				{
					handle->bsp_read_reg(0X65,&TempDat);  // 读取速度值
					if(TempDat == 0)
					{
						handle->bsp_write_reg(0X24,0);       // VMAX
						handle->bsp_write_reg(0X01,0X00);  // 清空限位
						handle->bsp_read_reg(0X21,&TempDat);  // 读取实际位置
						handle->bsp_write_reg(0X37,TempDat);   // 目标位置等于实际位置
//						Delayms(2);
						handle->bsp_write_reg(0X20,0X06);      //     位置模式
						handle->bsp_write_reg(0X24,PROTO_V_TO_4361_REG(handle->PROTO_V));       // VMAX
						handle->SPI_Status = handle->bsp_read_reg(0X36,&TempDat);
						handle->X_LATCH = (int32_t)TempDat;
						handle->XTARGET = handle->X_LATCH;
						handle->HOMEING_CMD = 0;
						handle->HOMING_FLAG = 1;
//						handle->PROTO_STATUS=(handle->PROTO_STATUS|M_STATUS_HOME_FINISH);
					}
				}	
			  break;
			default:;break;
		}
	}
}

void motor_poll(TypeDefTMC4361_RegStruct* handle)
{	
	
	uint32_t TempDat = 0;
	/*寄存器轮询更新*/
	HomingUpdate(handle);
	handle->SPI_Status = handle->bsp_read_reg(0X0E,&TempDat);
	handle->EVENTS = (int32_t)TempDat;
	if(/* handle->POLLING_FLAG == */1){
		handle->SPI_Status = handle->bsp_read_reg(0X50,&TempDat);
		handle->ENC_POS = (int32_t)TempDat;
		
	//	handle->SPI_Status = handle->bsp_read_reg(0X52,&TempDat);
	//	handle->ENC_POS_DEV = (int32_t)TempDat;
	//	
		handle->SPI_Status = handle->bsp_read_reg(0X59,&TempDat);
		handle->CL_OFFSET = (int32_t)TempDat;
		
		handle->SPI_Status = handle->bsp_read_reg(0X21,&TempDat);
		handle->XACTUAL = (int32_t)TempDat;
		
	//	handle->SPI_Status = handle->bsp_read_reg(0X37,&TempDat);

	//	handle->SPI_Status = handle->bsp_read_reg(0X36,&TempDat);
	//	handle->X_LATCH = (int32_t)TempDat;
		/*手册4.1 读取0x0E EVENT寄存器可以清除某些状态 所以这行不能删除!!!*/
		handle->SPI_Status = handle->bsp_read_reg(0X0E,&TempDat);
		handle->EVENTS = (int32_t)TempDat;

		handle->SPI_Status = handle->bsp_read_reg(0X0F,&TempDat);
		handle->STATUS = (int32_t)TempDat;
		
	//	handle->SPI_Status = handle->bsp_read_reg(0X01,&TempDat);
	//	handle->REFERENCE_CONF = (int32_t)TempDat;
		
		handle->SPI_Status = handle->bsp_read_reg(0X65,&TempDat);
		handle->V_ENC = (int32_t)TempDat;
		
		handle->SPI_Status = handle->bsp_read_reg(0X00,&TempDat); /*偶尔会出现该参数为0 导致电机加速度变为极低*/
		if(TempDat != 0x6026)
		{
			handle->bsp_write_reg(0X00,0x6026);
		}
	
		
		/*速度停止*/
		if((handle->STATUS & (VACTURAL_POSITIVE|VACTURAL_NEGETIVE)) == 0
#ifdef MOTOR_OPENLOOP
#else
			&&handle->V_ENC==0
#endif
		)
		{
			M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_STOP);
			
	//	到位检测
		if((ACTUAL_EQ_TAR & handle->STATUS))//&&handle->V_ENC==0&&abs(handle->ENC_POS - handle->XTARGET + handle->CL_OFFSET)<=500)
		{
			handle->stall_timer=0;
			M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_IN_POSITION);
		}
		
		}
		else
		{
			M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_STOP);
			M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_LEFT_TRIG);
			M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_RIGHT_TRIG);
		}
	}
/*
电机复位时朝光电反方向堵死	读出来的电机运动方向和编码器方向都是朝向光电位置的，实际上电机却反方向堵死了。
电机芯片初始化时在机械限位两端来回碰撞	没有发送运动指令，读出电机编码器的值正确的指示了电机的位置是在来回运动
*/
/*
电机复位时朝光电反方向堵死	读出来的电机运动方向和编码器方向都是朝向光电位置的，实际上电机却反方向堵死了。
电机芯片初始化时在机械限位两端来回碰撞	没有发送运动指令，读出电机编码器的值正确的指示了电机的位置是在来回运动
*/
	/*通过检测编码器寄存器防止碰撞发生*/
	if( 0
#ifdef MOTOR_OPENLOOP
//		|| (handle->STATUS & (STALL_DECTED | STALLGUARD)) != 0 /*出现电机在复位后运动速度变慢且报堵转错误*/
#else
		|| abs(handle->ENC_POS - handle->XACTUAL) > 10000 
		|| (ENC_OVER_TOL & handle->STATUS) != 0 /*电机来回运动堵死 编码器读数异常时这个错误位会置位*/
#endif
	)
	{
		if(handle->stall_tick == 0)
		{
			__disable_irq();
			handle->stall_tick = xTaskGetTickCount();
			__enable_irq();
		}
		else
		{
			uint32_t cur_tick;
			__disable_irq();
			cur_tick = xTaskGetTickCount();
			__enable_irq();
			if((cur_tick - handle->stall_tick) > 3000) /*持续三秒错误则上传错误*/
			{
//				motor_current(handle,0);
//				handle->bsp_write_reg(0X6D,0Xec);      // set CHOPCONF reg to initial value,release motor
//				handle->bsp_write_reg(0X6C,0x10410150); 
//				handle->bsp_nfreeze(1);
				M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_STALL);
//				M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_ERR);
			}
		}
	}
	else
	{
		M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_STALL);
		handle->stall_tick = 0;
	}
//	if(handle->PROTO_STATUS & M_STATUS_HOME_FINISH)
//	{
//		/*开始运动*/
//		if( (handle->PROTO_STATUS & M_STATUS_IN_POSITION) == 0 
//			&& handle->pos_status == POS_REACHED
//		)
//		{
//			handle->pos_status = POS_RUNNING;
//			__disable_irq();
//			handle->start_tick = xTaskGetTickCount();
//			__enable_irq();
//			
//		}
//		/*运动停止了*/
//		else if( (handle->PROTO_STATUS & M_STATUS_IN_POSITION) != 0 
//			&& handle->pos_status == POS_RUNNING
//		)
//		{
//			__disable_irq();
//			handle->stop_tick = xTaskGetTickCount();
//			__enable_irq();
//			handle->pos_status = POS_REACHED;
//			/*判断是否错误*/
//			/*短距离加减速过长 不判断*/
//			if(abs(handle->last_relative) < (51200 / 5))
//			{
//			}
//			else
//			{
//				/*平均速度 单位步数每秒*/
//				uint32_t avg_speed = 1000 * abs(handle->last_relative) / abs(handle->stop_tick - handle->start_tick);
//				handle->SPI_Status = handle->bsp_read_reg(0X24,&TempDat);
//				/*上位机将转每分钟 * 895 发送到下位机 下位机*256后写入0x24寄存器 */
//				//TempDat = TempDat / 256 / 895 * 51200 / 60;/*转单位 步数每秒*/
//				TempDat = TempDat / 895 / 60 * 200 ;
//				if(avg_speed < (TempDat / 10))
//				{
//					M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_SPEED_ERR);
//					M_STATUS_SET(&handle->PROTO_STATUS,M_STATUS_ERR);
//				}
//			}
//			
//		}
//	}
//	osDelay(1);
}
void motor_tarpos(TypeDefTMC4361_RegStruct* handle,int32_t tar)
{
	handle->last_relative = tar - handle->PROTO_XTATGET;
//	__disable_irq();
	uint32_t TempDat = 0;
	/*目标位置等于当前位置*/
//	if(handle->PROTO_XTATGET == tar)
//		return;
	handle->SPI_Status = handle->bsp_read_reg(0X36,&TempDat);
	handle->X_LATCH = (int32_t)TempDat;
	handle->PROTO_XTATGET = tar;
	handle->XTARGET = handle->X_LATCH + tar;

	handle->bsp_write_reg(0X37,handle->XTARGET);
	M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_IN_POSITION);
	M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_STOP);
}
void motor_relativepos(TypeDefTMC4361_RegStruct* handle,int32_t tar)
{
//	__disable_irq();
	uint32_t TempDat = 0;
	/*目标位置等于当前位置*/
//	if(handle->PROTO_XTATGET == tar)
//		return;
	handle->SPI_Status = handle->bsp_read_reg(0X36,&TempDat);
	handle->X_LATCH = (int32_t)TempDat;
	handle->PROTO_XTATGET = handle->PROTO_XTATGET + tar;
	handle->XTARGET = handle->X_LATCH + handle->PROTO_XTATGET;

	handle->bsp_write_reg(0X37,handle->XTARGET);
	M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_IN_POSITION);
	M_STATUS_RESET(&handle->PROTO_STATUS,M_STATUS_STOP);
}
void motor_speed(TypeDefTMC4361_RegStruct* handle,int32_t speed)
{
	handle->bsp_write_reg(0X24,PROTO_V_TO_4361_REG((speed * 895)));
	handle->bsp_write_reg(0X67,PROTO_V_TO_4361_REG((speed * 895)));
}